/*
 * T_Splitsing_microcontroller_code.c
 *
 * Created: 24-5-2013 10:59:30
 *  Author: Niels Markhorst
 */ 


#include <avr/io.h>
#include <avr/delay.h>
#include "T-Splitsing1.h"
#include "T-Splitsing2.h"
#include "globals.h"

void TWI_init_slave(void);
void TWI_match_read_slave(void);
void TWI_read_slave(void);
void TWI_match_write_slave(void);
void TWI_write_slave(void);

unsigned char write_data,recv_data;
unsigned char data = 0;
unsigned char Tjunction_Address = 0x20;

//general variable
int presentState1 = 1;
int presentState2 = 1;
int vertraging1;
int vertraging2;
int senArray1[3];
int senArray2[3];
int stoplichten1[3];
int stoplichten2[3];
uint8_t orByteA;
uint8_t orByteB;
uint8_t orByteC;
uint8_t orByteD;


uint8_t orBufferA;
uint8_t orBufferB;
uint8_t orBufferC;
uint8_t orBufferD;

int lichtArray[7][3] =
{
{1,1,1},
{3,1,1},
{2,1,1},
{1,3,1},
{1,2,1},
{1,1,3},
{1,1,2}
};


int mode;




int main(void)
{
	DDRB = 0xFF;
	PORTB = 0;
	_delay_ms(2000);
	
	//Make all Port C I/O pin outputs
	DDRC = (1<<PC7) | (1<<PC6) | (1<<PC5) | (1<<PC4) 
	| (1<<PC3) | (1<<PC2) | (1<<PC1) | (1<<PC0);
	//Set all Port C I/O pins low
	PORTC &= ~((1<<PC7) | (1<<PC6) | (1<<PC5) | (1<<PC4)
	| (1<<PC3) | (1<<PC2) | (1<<PC1) | (1<<PC0)); 
	
	DDRA = 0xC0;
	DDRB = 0x0F;
	DDRC = 0xFF;
	DDRD = 0xFC;
	//Prescale 64
	TCCR1B |= 0b00000011; 
	_delay_ms(500);
	TCNT1 = 0;
	TWI_init_slave();
    while(1)
    {
		//Loop tot correcte bevesteging is ontvangen
		while((TWSR & 0xF8)!= 0x60)  
	    {
		    //Get acknowlegement, Enable TWI, Clear TWI interrupt flag
		    TWCR=(1<<TWEA)|(1<<TWEN)|(1<<TWINT);
			//Wacht voor TWINT flag
		    while (!(TWCR & (1<<TWINT)))  
		    {
				if (TCNT1 > 7812)
				{
					if (vertraging1>0)
					{
						vertraging1--;
					}
					if (vertraging2>0)
					{
						vertraging2--;
					}
			
					TCNT1=0;
				}
				if (vertraging1==0)
				{
					orByteA = 0x00;
					orByteB = 0x00;
					orByteC &= 0b11111000;
					TSplitsing1();
				}			
				if (vertraging2==0)
				{
					orByteC &= 0b00000111;
					orByteD = 0x00;
					TSplitsing2();
				}	
				PORTA = orByteA;
				PORTB = orByteB;
				PORTC = orByteC;
				PORTD = orByteD;
			}
		}
	parse_data_tsplitsing();
	//debug
	//PORTB = 0; 
	_delay_ms(200);
	//debug
	//PORTB = ~0; 
	}
}

//Functie als er  een x aantal bytes ontvangen wordt dan moet de stoplicht data er uit halen en in een array stoppen
void parse_data_tsplitsing()
{

	unsigned char data = 0;
	unsigned char save = 0;
	int shift = 6;
	int data_saved = 0;
	int amount_read = 0;
	TWI_match_read_slave;
	//De eerste byte uit de TWI halen
	TWI_read_slave(); 
	//Dit is de herkennings byte, de eerste bit hiervan geeft aan welke mode het is
	mode = (recv_data & 1); 
	TWI_match_read_slave();
	//De tweede byte uit de TWI halen
	TWI_read_slave();
	//De data is de tweede byte 
	data = recv_data; 
	// 0 tm 6 (eerste as van de array)
	for(int i = 0; i < 7; i++) 
	{
		//0 tm 2 (tweede as van de array)
		for (int j = 0; j < 3; i++) 
		{
			//Shift data naar rechts, er hoeft maar 2 bit gebruikt te worden en die wordt op positie 1 en 2 gezet, hierna wordt het ge'& met 3 om de rest weg te halen
			save = ((data >> shift) & 3); 
			//Data wordt opgelsagen
			lichtArray[i][j] = save; 
			//Shift is het aantal plaatsen dat de data opgeshoven word, omdat de eerste twee bit helemaal links staan begint dit op 6, hierna moet het 4 worden, dan 2 en dan 0
			shift -=2; 
			//Bij -2 is de byte dus op en is alle data eruit gehaald die er in zat
			if(shift == -2) 
			{
				//Reset shift, voor de nieuwe byte
				shift = 6; 
				TWI_match_read_slave();
				//Nieuwe byte lezen
				TWI_read_slave(); 
				//Data is nieuwe byte
				data = recv_data; 
				//Aantal bytes er gelezen is ophogen
				amount_read++; 
			}
		}
	} 
	// als alles klaar is, zijn nog niet alle bytes gelezen omdat dit een T-splitsing is, er volgen nog een aantal bytes, dus totdat er 56 bytes (alle bytes die gelezen moeten worden) gelezen zijn
	while(amount_read < 57)	
	{
		//Lees alle bytes uit, om de buffer te legen
		amount_read++; 
		TWI_match_read_slave();
		TWI_read_slave();
	}
}

//Function to initilaize slave
void TWI_init_slave(void) 
{
	//Fill slave address to TWAR
	TWAR=Tjunction_Address;	
}

//Function to write data
void TWI_write_slave(void) 
{
	//Fill TWDR register whith the data to be sent
	TWDR= write_data;	
	//Enable TWI, Clear TWI interrupt flag          
	TWCR= (1<<TWEN)|(1<<TWINT);
	//Wait for the acknowledgement   
	while((TWSR & 0xF8) != 0xC0); 
}

//Function to match the slave address and slave dirction bit(write)
void TWI_match_write_slave(void) 
{
	//Loop till correct acknoledgement have been received
	while((TWSR & 0xF8)!= 0xA8)	
	{
		//Get acknowlegement, Enable TWI, Clear TWI interrupt flag
		TWCR=(1<<TWEA)|(1<<TWEN)|(1<<TWINT);
		//Wait for TWINT flag
		while (!(TWCR & (1<<TWINT)));  
	}
}

void TWI_read_slave(void)
{
	//TWI interrupt flag wissen,ontvang bevesteging, Enable TWI
	TWCR= (1<<TWINT)|(1<<TWEA)|(1<<TWEN);
	//Wacht voor TWINT flag
	while (!(TWCR & (1<<TWINT)));	
	//Wacht voor acknowledgement
	while((TWSR & 0xF8)!=0x80);		
	//Get waarde van TWDR
	recv_data=TWDR;					
	//PORTB=recv_data; //Zend de ontvange waarde op  PORTB				
	
}

///Functie om de slave address en slave directie bit(read) te vergelijken
void TWI_match_read_slave(void) 
{
	while((TWSR & 0xF8)!= 0x60)  // Loop till correct acknoledgement have been received
	{
		//ontvang bevesteging, Enable TWI, TWI interrupt flag wissen
		TWCR=(1<<TWEA)|(1<<TWEN)|(1<<TWINT);
		//Wacht voor TWINT flag
		while (!(TWCR & (1<<TWINT)));  
	}
}
